﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using System.Windows.Media.Imaging;
using System.IO;
using System.Windows.Media;
using System.Windows;
using System.ComponentModel;
using Biomedica.Graphics;
using System.Windows.Controls;
using NuiDicomPlayer.VirtualMouse;
using NuiDicomPlayer;
using System.Runtime.CompilerServices;

using NuiDicomPlayer.VirtualMouse.Kinect;
using Biomedica.Screen.Mappers;
using KinectWindows.Math;
using NuiDicomPlayer.DSP;
using KinectWindows;
using DTWGestureRecognition;
using System.Collections;
using System.Windows.Documents;




namespace KinectLibrary
{
    public delegate void DepthFrameReady(WriteableBitmap bmp);
    public delegate void JointFrameReady(Vector3 Lefthand, Vector3 RightHand, Vector3 ShoulderLeft, Vector3 ShoulderRight, Vector3 Spine, Vector3 Head, Vector3 Position);
    public delegate void GestureDetected(String Gesturename);
    /// <summary>
    /// This is the manager that will handle all the kineckt life cycle
    /// </summary>
    public class KinectGestureManager : IDisposable
    {
        #region SingletonConstructor
        protected static KinectGestureManager kinectGestureManager;
        public GestureDetected OnGestureDetected;
        [MethodImpl(MethodImplOptions.Synchronized)]
        public static KinectGestureManager New()
        {
            if (kinectGestureManager == null) return new KinectGestureManager();
            else return kinectGestureManager;
        }
        #endregion
        #region Proparties
        public Canvas canvas { set; get; }
        public Boolean EmitMultiSkeleton { set; get; }
        // public Window MainWindow{set;get;}
        public MouseController _mouseController { set; get; }
        public DepthFrameReady OnDepthFrameReady { set; get; }
        public bool MotorScanning { get; set; }
        private ScreencordMapper Screen_mapper;
        int framcount;
        Vector2 savedFilterdJointPostion;
        Vector2 savedTrend;
        Vector2 savedBasePostion;
        const int buffersize = 15;
        private ArrayList _video;
        public int MinimumFrames { get; set; }
        public bool _capturing { get; set; }
        public String Result { set; get; }
        public Filter _filterR = new Filter();
        public Filter _filterL = new Filter();


        DSP LH_kNormaliser_X = new DSP(buffersize);
        DSP RH_kNormaliser_X = new DSP(buffersize);
        DSP LH_kNormaliser_Y = new DSP(buffersize);
        DSP RH_kNormaliser_Y = new DSP(buffersize);

        public float TrendSmoothFactoe { set; get; }
        public float JitterRadious { set; get; }
        public float DataSmothingFactor { set; get; }
        public float PredicationFacrtor { set; get; }
        public float GlobalSmooth { set; get; }

        // GesturPostureEngine eng = new GesturPostureEngine();
        public List<Skeleton> SkeletonCollection
        {
            get;
            set;
        }
        public bool IsLeftHanded { get; set; }


        /// <summary>
        /// Set the Active Working Sensor
        /// </summary>
        public int ActiveSensor
        {
            set
            {
                if (value > KinectSensor.KinectSensors.Count)
                {
                    activesensor = -1;
                }
                else activesensor = value;
            }

            get
            {
                return activesensor;
            }
        }
        /// <summary>
        /// Enable The Active Kineckt Sensor
        /// </summary>
        public bool EnableKinect
        {
            set
            {
                if (value)
                {


                    if (ActiveSensor != -1)
                    {
                        var parameters = new TransformSmoothParameters
                        {
                            Correction = 0.9f,
                            Prediction = 0.5f,
                            Smoothing = 0.05f,
                            JitterRadius = 0.8f,
                            MaxDeviationRadius = 0.2f
                        };
                        KinectSensor.KinectSensors[ActiveSensor].SkeletonFrameReady += KinectGestureManager_SkeletonFrameReady;
                        KinectSensor.KinectSensors[activesensor].DepthFrameReady += sensor_DepthFrameReady;
                        KinectSensor.KinectSensors[ActiveSensor].DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);

                        KinectSensor.KinectSensors[ActiveSensor].SkeletonStream.Enable(parameters);
                        if (KinectSensor.KinectSensors[ActiveSensor].IsRunning)
                        KinectSensor.KinectSensors[ActiveSensor].Start();

                        Skeleton2DDataExtract.Skeleton2DdataCoordReady += NuiSkeleton2DdataCoordReady;
                        _capturing = false;
                        _video = new ArrayList();
                        BufferSize = 10000;
                        MinimumFrames = 6;
                    }
                    else
                    {
                        MessageBoxResult result = MessageBox.Show("There is no Kinect detected, please be sure that the kinect is connected and rightly configured, the program will shutdown now");
                        Environment.Exit(0);
                    }
                }
                else
                {
                    KinectSensor.KinectSensors[activesensor].DepthStream.Disable();
                    KinectSensor.KinectSensors[activesensor].SkeletonStream.Disable();
                    KinectSensor.KinectSensors[activesensor].Stop();
                }
            }
        }

        void sensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            var depth_frame = e.OpenDepthImageFrame();
            if (depth_frame != null)
            {
                DepthPixledata = new short[depth_frame.PixelDataLength];

                depth_frame.CopyPixelDataTo(DepthPixledata);

                if (depth_bitmap == null)
                {
                    depth_bitmap = new WriteableBitmap(depth_frame.Width, depth_frame.Height, 96, 96, PixelFormats.Bgr32, null);
                }

                depth_bytes = new byte[depth_frame.Width * depth_frame.Height * 4];
                ConvertToDepthImage(DepthPixledata, ref depth_bytes);
                int stride = depth_bitmap.PixelWidth * depth_bitmap.Format.BitsPerPixel / 8;
                Int32Rect rect = new Int32Rect(0, 0, depth_bitmap.PixelWidth, depth_bitmap.PixelHeight);
                depth_bitmap.WritePixels(rect, depth_bytes, stride, 0);

                // if (PropertyChanged!=null) PropertyChanged(this, new PropertyChangedEventArgs("depth_bitmap"));

            }
        }

        private void ConvertToDepthImage(short[] DepthPixledata, ref byte[] depth_data_32)
        {

            for (int i16 = 0, i32 = 0; i16 < DepthPixledata.Length && i32 < depth_data_32.Length; i16++, i32 += 4)
            {
                int user = DepthPixledata[i16] & 0x07;
                int realDepth = (DepthPixledata[i16] >> 3);
                byte intensity = (byte)(255 * realDepth / 0x1fff);

                switch (user)
                {
                    case (0):
                        if (realDepth > 1 && realDepth < 3)
                        {
                            depth_data_32[i32] = 0xff;
                            depth_data_32[i32 + 1] = 0x00;
                            depth_data_32[i32 + 2] = 0x00;
                            depth_data_32[i32 + 3] = intensity;
                        }
                        else
                        {
                            depth_data_32[i32] = 0x00;
                            depth_data_32[i32 + 1] = 0xff;
                            depth_data_32[i32 + 2] = 0xff;
                            depth_data_32[i32 + 3] = 0xff;
                        }

                        break;
                    case (1):
                        depth_data_32[i32] = (byte)(intensity);
                        break;
                    case (2):
                        depth_data_32[i32 + 1] = (byte)(intensity);
                        break;
                    case (3):
                        depth_data_32[i32 + 2] = (byte)(intensity);
                        break;
                    case (4):
                        depth_data_32[i32] = (byte)(intensity);
                        depth_data_32[i32 + 1] = (byte)(intensity);
                        break;
                    case (5):
                        depth_data_32[i32] = (byte)(intensity);
                        depth_data_32[i32 + 2] = (byte)(intensity);
                        break;
                }
            }



        }

        #endregion
        #region Feilds
        Int32 activesensor;
        byte[] depthFrame32;
        private short[] DepthPixledata;
        public WriteableBitmap depth_bitmap { set; get; }
        private byte[] depth_bytes;
        private Vector3 skelpos;
        private Vector3 left_hand;
        private Vector3 right_hand;
        private Vector3 head;
        private Vector3 spine;
        private Vector3 left_shoulder;
        private Vector3 right_shoulder;
        private Vector3 center_shoulder;
        #endregion
        #region delegates
        public JointFrameReady OnJointFrameReady { set; get; }
        #endregion


        /// <summary>
        /// Construct a new Kineckt Sensor Manager
        /// </summary>
        public KinectGestureManager()
        {

            // eng.Limit = 60;
            TrendSmoothFactoe = 0.25f;
            JitterRadious = 0.05f;
            DataSmothingFactor = 0.5f;
            PredicationFacrtor = 0.5f;
            savedFilterdJointPostion = Vector2.Zero;
            savedTrend = Vector2.Zero;
            savedBasePostion = Vector2.Zero;
            framcount = 0;

            //   right_handBuffer = new Biomedica.DSP.Buffer(eng.Limit);
            //   left_handBuffer = new Biomedica.DSP.Buffer(eng.Limit);
            //   spine_Buffer = new Biomedica.DSP.Buffer(eng.Limit);
            //   head_Buffer = new Biomedica.DSP.Buffer(eng.Limit);
            //   skeleton_pos_buffer = new Biomedica.DSP.Buffer(eng.Limit);
            //  com_bo_buffer = new Biomedica.DSP.Buffer(eng.Limit);
        }
        /// <summary>
        /// Triggerd when Skeleton Fram is ready
        /// </summary>
        /// <param name="sender">The sender Object</param>
        /// <param name="e">Events Parameters</param>
        void KinectGestureManager_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            using (SkeletonFrame frame = e.OpenSkeletonFrame())
            {
                if (frame != null)
                {
                    Skeleton[] skeletons = new Skeleton[frame.SkeletonArrayLength];
                    frame.CopySkeletonDataTo(skeletons);
                    
                    if (skeletons.Length > 0) AnalyseSkeleton(skeletons);
                    frame.Dispose();
                }
            }
        }


        /// <summary>
        /// Set the Sensot angle in any value between -30 and 30
        /// </summary>
        /// <param name="angle">The angle to til</param>
        public void SetSensorTilt(double angle)
        {
            if (angle > -30 && angle < 30)
                KinectSensor.KinectSensors[activesensor].ElevationAngle = (int)angle;
        }
        /// <summary>
        /// Perform Skeleton Analaysis
        /// </summary>
        /// <param name="skeletons">Collection of all the skeletons in the stage</param>
        private void AnalyseSkeleton(Skeleton[] skeletons)
        {

            for (int i = 0; i < skeletons.Length; i++)
            {
                if (skeletons[i].TrackingState == SkeletonTrackingState.Tracked)
                {
                    #region Calculate JointPostions

                    skelpos = new Vector3(skeletons[i].Position.X, skeletons[i].Position.Y, skeletons[i].Position.Z);
                    left_hand = (new Vector3(skeletons[i].Joints[JointType.HandLeft].Position.X,
                                                  skeletons[i].Joints[JointType.HandLeft].Position.Y, skeletons[i].Joints[JointType.HandLeft].Position.Z));
                    right_hand = (new Vector3(skeletons[i].Joints[JointType.HandRight].Position.X,
                                                 skeletons[i].Joints[JointType.HandRight].Position.Y, skeletons[i].Joints[JointType.HandRight].Position.Z));
                    head = (new Vector3(skeletons[i].Joints[JointType.Head].Position.X,
                                                 skeletons[i].Joints[JointType.Head].Position.Y, skeletons[i].Joints[JointType.HandRight].Position.Z));
                    spine = (new Vector3(skeletons[i].Joints[JointType.Spine].Position.X,
                                                skeletons[i].Joints[JointType.Spine].Position.Y, skeletons[i].Joints[JointType.HandRight].Position.Z));
                    left_shoulder = (new Vector3(skeletons[i].Joints[JointType.ShoulderLeft].Position.X,
                                           skeletons[i].Joints[JointType.ShoulderLeft].Position.Y, skeletons[i].Joints[JointType.HandRight].Position.Z));
                    right_shoulder = (new Vector3(skeletons[i].Joints[JointType.ShoulderRight].Position.X,
                                           skeletons[i].Joints[JointType.ShoulderRight].Position.Y, skeletons[i].Joints[JointType.HandRight].Position.Z));

                    center_shoulder = (new Vector3(skeletons[i].Joints[JointType.ShoulderCenter].Position.X,
                                      skeletons[i].Joints[JointType.ShoulderCenter].Position.Y, skeletons[i].Joints[JointType.HandRight].Position.Z));

                    // Update();




                    Skeleton2DDataExtract.ProcessData(skeletons[i]);
                    ///////////////////////////////
                    OnJointFrameReady(left_hand, right_hand, left_shoulder, right_shoulder, spine, head, skelpos);
                    #endregion

                    break;
                }
            }
        }
        public Vector2 FilterJointPostion(Vector2 Joint)
        {
            var sensor = Microsoft.Kinect.KinectSensor.KinectSensors[activesensor];
            Vector2 filteredJointPostion = Vector2.Zero;
            Vector2 differanceVector = Vector2.Zero;
            Vector2 currentTrend = Vector2.Zero;
            Vector2 baseJointPostion = Joint;
            Vector2 previousFilterdJointPostion = savedFilterdJointPostion;
            Vector2 previousTrens = savedTrend;

            Vector2 previousBaseJointPostion = savedBasePostion;

            switch (framcount)
            {
                case (0):
                    filteredJointPostion = baseJointPostion;
                    currentTrend = Vector2.Zero;
                    break;
                case (1):
                    filteredJointPostion = (baseJointPostion + previousBaseJointPostion) * 0.5;
                    differanceVector = filteredJointPostion - previousBaseJointPostion;
                    currentTrend = differanceVector * TrendSmoothFactoe + previousTrens * (1.0f - TrendSmoothFactoe);
                    break;

                default:

                    differanceVector = baseJointPostion - previousFilterdJointPostion;


                    float distance = differanceVector.Distance();
                    if (distance <= JitterRadious)
                        filteredJointPostion = baseJointPostion * (distance / JitterRadious) + previousFilterdJointPostion * (1.0f) - (distance / JitterRadious);
                    else
                        filteredJointPostion = baseJointPostion;

                    filteredJointPostion = filteredJointPostion * (1.0f - DataSmothingFactor) + (previousFilterdJointPostion + previousTrens) * DataSmothingFactor;
                    differanceVector = filteredJointPostion - previousFilterdJointPostion;
                    currentTrend = differanceVector * TrendSmoothFactoe + previousTrens * (1.0f - TrendSmoothFactoe);
                    break;

            }
            Vector2 potentialNewPOstion = new Vector2(filteredJointPostion.X + currentTrend.X * PredicationFacrtor, filteredJointPostion.Y + currentTrend.Y * PredicationFacrtor);
            savedBasePostion = baseJointPostion;
            savedFilterdJointPostion = filteredJointPostion; ;
            savedTrend = currentTrend;
            framcount++;
            return potentialNewPOstion;

        }

        public void Dispose()
        {
            Dispose(true);

        }
        private void Dispose(bool disposing)
        {
            if (disposing)
            {

                if (this != null)
                {
                    KinectSensor.KinectSensors.Dispose();

                }
            }
        }


        public void Update()
        {
            //{

            #region mouse tracking

            Vector2 v1 = Vector2.Zero;
            Vector2 v2 = Vector2.Zero;
            Screen_mapper = new ScreencordMapper(canvas);
            Vector3 cog_postion = Vector3.Zero;
            Screen_mapper.SmothingFactor = 1.2;

            //  if (!IsLeftHanded) cog_postion = left_shoulder;
            //  else cog_postion = right_shoulder;
            cog_postion = center_shoulder;

            #region Mapcord to screen Cord
            Screen_mapper.MapKinecktToScreen(cog_postion, left_hand);
            left_hand.X = (float)Screen_mapper.X;
            left_hand.Y = (float)Screen_mapper.Y;

            Screen_mapper.MapKinecktToScreen(cog_postion, right_hand);
            right_hand.X = (float)Screen_mapper.X;
            right_hand.Y = (float)Screen_mapper.Y;

            //Screen_mapper.MapScreenToScreen(cog_postion.X, cog_postion.Y, head.X, head.Y);
            //head.X = (float)Screen_mapper.X;
            //head.Y = (float)Screen_mapper.Y;


            //Screen_mapper.MapScreenToScreen(cog_postion.X, cog_postion.Y, spine.X, spine.Y);
            //spine.X = (float)Screen_mapper.X;
            //spine.Y = (float)Screen_mapper.Y;

            //Screen_mapper.MapScreenToScreen(cog_postion.X, cog_postion.Y, left_shoulder.X, left_shoulder.Y);
            //left_shoulder.X = (float)Screen_mapper.X;
            //left_shoulder.Y = (float)Screen_mapper.Y;


            //Screen_mapper.MapScreenToScreen(cog_postion.X, cog_postion.Y, right_shoulder.X, right_shoulder.Y);
            //right_shoulder.X = (float)Screen_mapper.X;
            //right_shoulder.Y = (float)Screen_mapper.Y;

            //Screen_mapper.MapScreenToScreen(cog_postion.X, cog_postion.Y, center_shoulder.X, center_shoulder.Y);
            //center_shoulder.X = (float)Screen_mapper.X;
            //center_shoulder.Y = (float)Screen_mapper.Y;

            #endregion
            #region update hand postions on screen
            v2 = _filterL.FilterJointPostion(Microsoft.Kinect.KinectSensor.KinectSensors[activesensor], Vector2.XYFromVector3(left_hand));
            v1 = _filterR.FilterJointPostion(Microsoft.Kinect.KinectSensor.KinectSensors[activesensor], Vector2.XYFromVector3(right_hand));


            NuiEventManager.PostEvent(new NuiEvent(KinectWindows.Messages.WindowMessages.NUI_MSG_LEFT_HAND_MOVED, new Object[] { v2 }));
            NuiEventManager.PostEvent(new NuiEvent(KinectWindows.Messages.WindowMessages.NUI_MSG_RIGHT_HAND_MOVED, new Object[] { v1 }));

            #endregion
            #endregion

            #region Detection Cycles and filters

            //App.Nav.StartPostureDetectionCycle(head, spine, left_hand, right_hand, right_shoulder, left_hand, center_shoulder, skelpos);

            #endregion
        }

        private void NuiSkeleton2DdataCoordReady(object sender, Skeleton2DdataCoordEventArgs a)
        {

           

            // We need a sensible number of frames before we start attempting to match gestures against remembered sequences
            if (_video.Count > MinimumFrames && _capturing == false)
            {
                ////Debug.WriteLine("Reading and video.Count=" + video.Count);
                Result = WindowManager.WindowManagerLooper.GrabHand.GestureRecogniser.Recognize(_video);

                if (!Result.Contains("__UNKNOWN"))
                {
                    // There was no match so reset the buffer
                    _video = new ArrayList();
                    if (OnGestureDetected != null) OnGestureDetected(Result);
                    
                    
                }
            }

            // Ensures that we remember only the last x frames
            if (_video.Count > BufferSize)
            {
                // If we are currently capturing and we reach the maximum buffer size then automatically store
                if (_capturing)
                {
                   // DtwStoreClick(null, null);
                }
                else
                {
                    // Remove the first frame in the buffer
                    _video.RemoveAt(0);
                }
            }

            // Decide which skeleton frames to capture. Only do so if the frames actually returned a number. 
            // For some reason my Kinect/PC setup didn't always return a double in range (i.e. infinity) even when standing completely within the frame.
            // TODO Weird. Need to investigate this
            if (!double.IsNaN(a.GetPoint(0).X))
            {
                // Optionally register only 1 frame out of every n
                _flipFlop = (_flipFlop + 1) % Ignore;
                if (_flipFlop == 0)
                {
                    _video.Add(a.GetCoords());
                }
            }

            // Update the debug window with Sequences information
            //dtwTextOutput.Text = _dtw.RetrieveText();

        }
        public int BufferSize { get; set; }
        private int _flipFlop;
        private const int Ignore = 2;
    }
}
